
#ifndef CARTOGRAPHER_MAPPING_2D_GRID_2D_H_
#define CARTOGRAPHER_MAPPING_2D_GRID_2D_H_

#include <cstdint>
#include <vector>
#include <memory>
#include <set>
#include <mutex>

#include "third_party/include/safe_queue.h"
#include "mapping/2d/map_limits.h"

namespace cartographer {
namespace mapping {
typedef std::recursive_mutex mutex_t;

class Grid2D {
public:
    explicit Grid2D(const MapLimits &limits, float min_correspondence_cost, float max_correspondence_cost);

    Grid2D(const Grid2D &grid);  // 用于拷贝grid

    // Returns the limits of this Grid2D.
    MapLimits limits() const { return limits_; }

    // Finishes the update sequence.
    void FinishUpdate();

    // Returns the correspondence cost of the cell with 'cell_index'.
    float GetCorrespondenceCost(const Eigen::Array2i &cell_index) const;

    // Returns the minimum possible correspondence cost.
    float GetMinCorrespondenceCost() const { return min_correspondence_cost_; }

    // Returns the maximum possible correspondence cost.
    float GetMaxCorrespondenceCost() const { return max_correspondence_cost_; }

    // Returns true if the probability at the specified index is known.
    bool IsKnown(const Eigen::Array2i &cell_index) const;

    bool IsOutOfBounds(const Eigen::Array2i &cell_index) const;

    // Fills in 'offset' and 'limits' to define a subregion of that contains all known cells.
    void ComputeCroppedLimits(Eigen::Array2i *offset, CellLimits *limits) const;

    // Grows the map as necessary to include 'point'. This changes the meaning of
    // these coordinates going forward. This method must be called immediately
    // after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
    virtual void GrowLimits(const Eigen::Vector2f &point);

    virtual std::unique_ptr<Grid2D> ComputeCroppedGrid() = 0;

    const std::vector<uint16_t> &GetCorrespondenceCostCells() const { return correspondence_cost_cells_; }

    SafeQueue<Eigen::Array2i> &GetUpdateGridCellsQueue() { return update_grid_cells_queue_; }

    mutex_t *cells_mutex() { return &cells_mutex_; };

protected:
    const std::vector<uint16_t> &correspondence_cost_cells() const { return correspondence_cost_cells_; }

    std::vector<uint16_t> *mutable_correspondence_cost_cells() { return &correspondence_cost_cells_; }

    std::vector<int> *mutable_update_indices() { return &update_indices_; }

    const Eigen::AlignedBox2i &known_cells_box() const { return known_cells_box_; }

    Eigen::AlignedBox2i *mutable_known_cells_box() { return &known_cells_box_; }

    // Converts a 'cell_index' into an index into 'cells_'.
    int ToFlatIndex(const Eigen::Array2i &cell_index) const;

    mutable mutex_t cells_mutex_;

    // private:

    MapLimits             limits_;
    std::vector<uint16_t> correspondence_cost_cells_;  // 保存了栅格坐标的代价
    float                 min_correspondence_cost_;
    float                 max_correspondence_cost_;
    std::vector<int>      update_indices_;
    // Bounding box of known cells to efficiently compute cropping limits.
    Eigen::AlignedBox2i
        known_cells_box_;  // 保存了栅格坐标的bounding box, 和真实坐标点差了翻转和旋转180,外加一个resolution

    SafeQueue<Eigen::Array2i> update_grid_cells_queue_;
};

}  // namespace mapping
}  // namespace cartographer

#endif  // CARTOGRAPHER_MAPPING_2D_GRID_2D_H_
